int PrevDir=Forward;
int m1PrevSpd=0;
int m2PrevSpd=0;
double m1Skip=0;
double m1Skip2=0;
double m2Skip=0;
double m2Skip2=0;


#define Acceleration_Value 1
#define Addition_Skipper 50


#define		left	0
#define		right	1

int M1Last=0;
int M2Last=0;
int M1LastDir=Forward;
int M2LastDir=Forward;

void go(int m1dir, int m1spd, int m2dir, int m2spd) // speed switch function
{
	M1Last=m1spd;
	M2Last=m2spd;
	M1LastDir=m1dir;
	M2LastDir=m2dir;
	switch (m1dir)	//Motor One
	{
		case 1:		//Forward
			M1C2 = 255-Acceleration(left,m1spd);	//Set Speed
			M1C1 = ON;		//Set Direction
			break;
		case 2:		//Backward
			M1C2 = Acceleration(left,m1spd);	//Set Speed
			M1C1 = OFF;		//Set Direction
			break;
		default:	//Bad Direction
			LCDdispMSGNEW("BAD DIRECTION MOTOR 1",21);
			
	}
	switch (m2dir)	//Motor One
	{
		case 1:		//Forward
			M2C2 = Acceleration(right,m2spd);	//Set Speed
			M2C1 = OFF;		//Set Direction
			break;
		case 2:		//Backward
			M2C2 = 255-Acceleration(right,m2spd);	//Set Speed
			M2C1 = ON;		//Set Direction
			break;
		default:	//Bad Direction
			LCDdispMSGNEW("BAD DIRECTION MOTOR 2",21);
			
	}
}

int Acceleration(int WMotor, int Speed_Wanted) //This function is used to accelerate
{
	return Speed_Wanted;
/*
	int ReturnSpeed=0;
	switch (WMotor)
	{
		case left:
			if (Speed_Wanted>m1PrevSpd)	//Need to do acceleration
			{
				if (m1Skip==Addition_Skipper)
				{
					m1PrevSpd+=Acceleration_Value;
					m1Skip=0;
				}
				else
					m1Skip++;
			
			}
			else
			{
				ReturnSpeed= m1PrevSpd;
				m1Skip=0;
			}
			break;
		case right:
			if (Speed_Wanted>m2PrevSpd)	//Need to do acceleration
			{
				if (m2Skip==Addition_Skipper)
				{					
					m2PrevSpd+=Acceleration_Value;
					m2Skip=0;
				}
				else
					m2Skip++;
			}	
			else
			{
				m2PrevSpd=Speed_Wanted;
				m2Skip=0;
			}
			ReturnSpeed= m2PrevSpd;
			break;
		default:
			USART1writeln("Bad Motor Direction in Acceleration Function");
	}
	return ReturnSpeed;	//shouldn't ever reach here actually
*/

}

void stop(void)
{
	go(Backward,0,Backward,0);	//Stop Motors

}


void goforward(int speed) //forward motion function
{

	go(Forward,speed,Forward,speed);
}
void goleft(int speed) //Tank turn left
{
	go(Backward,speed,Forward,speed);
}
void goright(int speed) //Tank turn right
{
	go(Forward,speed,Backward,speed);

}
void gobackward(int speed) //backward motion funtion
{
	go(Backward,speed,Backward,speed);

}

void InitMotors()
{
	// set up PB0 and PB1 with 8bit PWM
	TCCR1A = _BV(WGM10)  // 8 BIT, PHASE CORRECT PWM
		   | _BV(COM1A1)
		   | _BV(COM1B1);
	TCCR1B = _BV(CS10)   // NO PRESCALE
		   | _BV(WGM12);  // FAST PWM
		   
}
